#include <iostream>

#include <ctime>
// Eigen 核心部分
// 稠密矩阵的代数运算 (逆、特征值等)
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>

const size_t MATRIX_SIZE = 50UL;
// #define MATRIX_SIZE = 50;

/*********************************
 * 本程序演示了 Eigen 基本类型的使用
 ********************************/

int main (int argv, char** argc)
{
  // Eigen 中所有向量和矩阵都是 Eigen::Matrix, 它是一个模板类。他的前三个参数为数据类型、行数、列数
  // 声明一个 2*3 的 float 矩阵
  Eigen::Matrix<float , 2, 3> matrix_23;

  // 同时，Eigen 通过 typedef 提供了许多内置类型，不过底层仍是 Eigen::Matrix
  // 例如，Vector3d 实质上是 Eigen::Matrix<double, 3, 1>, 即三维向量
  Eigen::Vector3d v_3d;
  // 这和上面是一样的
  Eigen::Matrix<double, 3, 1> vd_3d;

  // Matrix3d 实质上是 Eigen::Matrix<double, 3, 3>
  Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Zero(); // 初始化为零
  // 如果不确定矩阵大小，可以使用动态大小的矩阵
  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> matrix_dynamic;
  // 更推荐使用 typedef
  Eigen::MatrixXd matrix_x;
  // 这种类型还有很多，不在此一一列举

  // 下面试对 Eigen 阵的操作
  // 输入数据 (初始化
  matrix_23 << 1, 2, 3, 4, 5, 6;

  std::cout << " matrix 2x3 from 1 to 6: \n" << matrix_23 << std::endl;

  // 用 () 选取矩阵中的元素
  std::cout << "matrix 2x3：" << std::endl;
  for (int i = 0; i < 2; ++i)
  {
    for (int j = 0; j < 3; ++j) std::cout << matrix_23(i, j) << "\t";
    std::cout << std::endl;
  }

  // 矩阵和向量相乘 (实际上仍是矩阵和矩阵)
  v_3d << 3, 2, 1;
  vd_3d << 4, 5, 6;

  // 但是在 Eigen 里不能混合两种不同类型的矩阵， 像这样是错的
  // Eigen::Matrix<double, 2, 1> result_wrong_type = matrix_23 * v_3d;
  // 应该显式转换
  Eigen::Matrix<double, 2, 1> result = matrix_23.cast<double>() * v_3d;
  std::cout << "[1, 2, 3, 4, 5, 6] * [3, 2, 1] = " << result.transpose() << std::endl;

  // Eigen::Matrix<float, 2, 1> result2 = matrix_23 * v_3d;
  // std::cout << "[1, 2, 3, 4, 5, 6] * [4, 5, 6] = " << result2.transpose() << std::endl;

  // 同样，不可以搞错矩阵的维度
  // 下面的注释是错误的
  // Eigen::Matrix<double, 2, 3> result_wrong_dimension = matrix_23.cast<double>() * v_3d;

  // 但是这样是可以的
  // Eigen::Matrix<double, 2, 1> result_right_dimension = matrix_23.cast<double>() * vd_3d;

  // 一些矩阵运算
  // 四则运算就不演示了， 直接用 + - * / 即可
  matrix_33 = Eigen::Matrix3d::Random(); // 随机数矩阵
  std::cout << "random matrix: \n" << matrix_33 << std::endl;
  std::cout << "transpose: \n" << matrix_33.transpose() << std::endl; // 转置
  std::cout << "sum: " << matrix_33.sum() << std::endl;               // 各元素和
  std::cout << "trace: " << matrix_33.trace() <<std::endl;            // 迹
  std::cout << "times 10: \n" << 10 * matrix_33 << std::endl;         // 数乘 
  std::cout << "inverse: \n" << matrix_33.inverse() << std::endl;     // 逆
  std::cout << "det: " << matrix_33.determinant() << std::endl;       // 行列式

  // 特征值
  // 实对称矩阵可以保证对角化成功
  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigen_solver(matrix_33.transpose() * matrix_33);
  std::cout << "Eigen values = \n" << eigen_solver.eigenvalues() << std::endl;
  std::cout << "Eigen values = \n" << eigen_solver.eigenvectors() << std::endl;

  // 解方程
  // 求解 matrix_MN * x = v_Nd 方程
  // N 的大小在前面的宏定义中，它由随机数生成
  // 直接求逆自然是最直接的，但是运算量大

  Eigen::Matrix<double, MATRIX_SIZE, MATRIX_SIZE> matrix_NN
    = Eigen::MatrixXd::Random(MATRIX_SIZE, MATRIX_SIZE);
  matrix_NN = matrix_NN * matrix_NN.transpose();  // 保证半正定
  Eigen::Matrix<double, MATRIX_SIZE, 1> v_Nd = Eigen::MatrixXd::Random(MATRIX_SIZE, 1);

  clock_t time_stt = clock();
  // 直接求逆
  Eigen::Matrix<double, MATRIX_SIZE, 1> x = matrix_NN.inverse() * v_Nd;
  std::cout << "time of normal inverse is"
            << 1000 * (clock() - time_stt) / (double) CLOCKS_PER_SEC << "ms" << std::endl;
  std::cout << "x = " << x.transpose() << std::endl;


  // 通常用矩阵分解来求解， 如 QR 分解，速度会快很多
  time_stt = clock();
  x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
  std::cout << "time of Qr decomposition is"
            << 1000 * (clock() - time_stt) / (double) CLOCKS_PER_SEC << "ms" << std::endl;
  std::cout << "x = " << x.transpose() << std::endl;
  
  // 对于正定矩阵，还可以用 cholesky 分解来解方程
  time_stt = clock();
  x = matrix_NN.ldlt().solve(v_Nd);
  std::cout << "time of ldlt decomposition is"
            << 1000 * (clock() - time_stt) / (double) CLOCKS_PER_SEC << "ms" << std::endl;

  return 0;
}